//
//  main.cpp
//  RotatingCube
//
//  Created by Peter Horak on 9/3/12.
//  Copyright (c) 2012 Peter Horak. All rights reserved.
//

#include <cstdlib>
#include <cmath>
#include <GLUT/GLUT.h>

#include <SFML/Graphics.hpp>

#include "math_3d.h"

#include <fstream>
#include <sstream>
#include <iostream>
using namespace std;


int ww = 750;
int wh = 750;

GLfloat vertices[8][3] = {{-2.0,-1.0,-1.0},{2.0,-1.0,-1.0},{2.0,1.0,-1.0},{-2.0,1.0,-1.0},{-2.0,-1.0,1.0},
    {2.0,-1.0,1.0},{2.0,1.0,1.0},{-2.0,1.0,1.0}};

static GLdouble v_r = 5;
static GLdouble v_theta = 0;
static GLdouble v_fi = 0;

GLuint tex[7];

static GLfloat light_pos[] = {3.0, 3.0, 10.0, 1.0};

ifstream fin;

Vector3f mag_world(4.9858,-18.1809,-49.7536);
Vector3f acc_world(0,0,1);

Vector3f acc(0,0,0);
Vector3f mag(0,0,0);

float rotation1;
Vector3f axis1;
float rotation2;
Vector3f axis2;

static float curr_time = 0;
static int d0,d1,d2,d3;

void polygon(int a, int b, int c, int d)
{
    GLfloat u[3];
    for (int i=0;i<3;i++) u[i] = vertices[a][i] - vertices[c][i];
    GLfloat v[3];
    for (int i=0;i<3;i++) v[i] = vertices[b][i] - vertices[c][i];
    GLfloat n[3] = {u[1]*v[2]-u[2]*v[1],u[2]*v[0]-u[0]*v[2],u[0]*v[1]-u[1]*v[0]};
    GLfloat len = sqrt(n[0]*n[0]+n[1]*n[1]+n[2]*n[2]);
    for (int i=0;i<3;i++) n[i] = n[i]/len;
    
    glNormal3fv(n);
    
    glBegin(GL_POLYGON);
    glTexCoord2f(0.0, 0.0);
    glVertex3fv(vertices[a]);
    glTexCoord2f(1.0, 0.0);
    glVertex3fv(vertices[b]);
    glTexCoord2f(1.0, 1.0);
    glVertex3fv(vertices[c]);
    glTexCoord2f(0.0, 1.0);
    glVertex3fv(vertices[d]);
    glEnd();
}

void colorcube()
{
    glBindTexture(GL_TEXTURE_2D, tex[0]);
    polygon(1,0,3,2); // bottom
    glBindTexture(GL_TEXTURE_2D, tex[1]);
    polygon(7,6,2,3); // side
    glBindTexture(GL_TEXTURE_2D, tex[2]);
    polygon(3,0,4,7); // end
    glBindTexture(GL_TEXTURE_2D, tex[3]);
    polygon(5,1,2,6); // end
    glBindTexture(GL_TEXTURE_2D, tex[4]);
    polygon(6,7,4,5); // top
    glBindTexture(GL_TEXTURE_2D, tex[5]);
    polygon(5,4,0,1); // side
}

void display()
{
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    glLoadIdentity();
    gluLookAt(v_r*cos(v_theta)*cos(v_fi),v_r*sin(v_theta)*cos(v_fi),v_r*sin(v_fi),0,0,0,0,0,1);
    
    glDisable(GL_TEXTURE_2D);
    glDisable(GL_LIGHTING);
    
    // Overlay text
    glMatrixMode(GL_PROJECTION);
    glPushMatrix();
    glLoadIdentity();

    gluOrtho2D(0.0, ww, 0.0, wh);
    
    glMatrixMode(GL_MODELVIEW);
    glPushMatrix();
    glLoadIdentity();
    
    glColor3f(1.0, 0.0, 0.5); // Green
    int line = wh - 20;
    glRasterPos2i(10, line);

    ostringstream ss;
    ss.precision(2);
    ss << fixed << "Time: " << (curr_time-52678.0) << "_D0: " << d0 << "_D1: " << d1 << "_D2: " << d2 << "_D3: " << d3;
    string s = ss.str();
    void * font = GLUT_BITMAP_9_BY_15;
    for (string::iterator i = s.begin(); i != s.end(); ++i)
    {
        char c = *i;
        if (c == '_')
        {
            glColor3f(0.0, 1.0, 0.0);
            line -= 20;
            glRasterPos2i(10, line);
        }
        else
            glutBitmapCharacter(font, c);
    }
    
    
    glMatrixMode(GL_MODELVIEW);
    glPopMatrix();
    
    glMatrixMode(GL_PROJECTION);
    glPopMatrix();
    glMatrixMode(GL_MODELVIEW);
    
    
    // Draw vector lines
    glColor3d(1, 0, 0);
    glBegin(GL_LINE_LOOP);
    glVertex3f(0, 0, 0);
    glVertex3f(3*acc_world.x,3*acc_world.y,3*acc_world.z);
    glEnd();
    
    glColor3d(0, 0, 1);
    glBegin(GL_LINE_LOOP);
    glVertex3f(0, 0, 0);
    glVertex3f(3*mag_world.x,3*mag_world.y,3*mag_world.z);
    glEnd();
    
    glColor3d(1, 0.5, 0);
    glBegin(GL_LINE_LOOP);
    glVertex3f(0, 0, 0);
    glVertex3f(3*acc.x,3*acc.y,3*acc.z);
    glEnd();
    
    glColor3d(0.5, 0, 1);
    glBegin(GL_LINE_LOOP);
    glVertex3f(0, 0, 0);
    glVertex3f(3*mag.x,3*mag.y,3*mag.z);
    glEnd();
    
    glEnable(GL_LIGHTING);
    glEnable(GL_TEXTURE_2D);
    
    glLightfv(GL_LIGHT0, GL_POSITION, light_pos);
    
    glBindTexture(GL_TEXTURE_2D, tex[6]);
    glNormal3f(0, 0, 1);
    glBegin(GL_POLYGON);
    glColor3f(0.3, 0.3, 0.3);
    glTexCoord2f(0.0, 0.0);
    glVertex3f(20, -20, -2);
    glTexCoord2f(10.0, 0.0);
    glVertex3f(20, 20, -2);
    glTexCoord2f(10.0, 10.0);
    glVertex3f(-20, 20, -2);
    glTexCoord2f(0.0, 10.0);
    glVertex3f(-20, -20, -2);
    glEnd();
    
    Vector3f vectors[8] = {Vector3f(-2.0,-1.0,-1.0),Vector3f(2.0,-1.0,-1.0),Vector3f(2.0,1.0,-1.0),Vector3f(-2.0,1.0,-1.0),Vector3f(-2.0,-1.0,1.0),Vector3f(2.0,-1.0,1.0),Vector3f(2.0,1.0,1.0),Vector3f(-2.0,1.0,1.0)};
    for (int i = 0; i < 8; i++)
    {
        vectors[i].Rotate(rotation1, axis1);
        vectors[i].Rotate(rotation2, axis2);
        vertices[i][0] = vectors[i].x;
        vertices[i][1] = vectors[i].y;
        vertices[i][2] = vectors[i].z;
    }
    
    colorcube();
    
    glFlush();
    glutSwapBuffers();
}

void spinCube(int n)
{
    float lat, lng, alt;
    int ax,ay,az,mx,my,mz;
    
    fin >> lat >> lng >> alt >> ax >> ay >> az >> mx >> my >> mz >> d0 >> d1 >> d2 >> d3;
    //string line;
    //getline(inputFile,line);
    //sscanf(line.c_str(), " %f %f %f %d %d %d %d %d %d %d %d %d %d",&lat,&lng,&alt,&az,&ax,&ay,&mz,&mx,&my,&d0,&d1,&d2,&d3);
    
    acc = Vector3f(-az,ay,ax);
    mag = Vector3f(-mz,my,mx);
    acc.Normalize();
    mag.Normalize();
    
    Vector3f perp = mag.Cross(acc);
    mag = acc.Cross(perp);
    mag.Normalize();
    
    rotation1 = Angle(acc, acc_world);
    axis1 = Axis(acc, acc_world);
    mag.Rotate(rotation1, axis1);
    rotation2 = Angle(mag, mag_world);
    axis2 = Axis(mag, mag_world);//acc_world
    
    /*
    rotation1 = Angle(mag, mag_world);
    axis1 = Axis(mag, mag_world);
    acc.Rotate(rotation1, axis1);
    rotation2 = Angle(acc, acc_world);
    axis2 = mag_world;*/
    
    mag.Rotate(-rotation1, axis1); // undo rotation
    
    float next_time;
    fin >> next_time;
    float prev_time = curr_time;
    curr_time = next_time;
    
    glutPostRedisplay();
    glutTimerFunc(1000*(next_time-prev_time), spinCube, n);//1000*(next_time-prev_time)
}

void keysDown(unsigned char key, int x, int y)
{
    if (key=='q' or key=='Q') exit(0);
    
    double speed = 64.0;
    if (key=='w')
    {
        v_fi += M_PI/speed;
        if (v_fi > M_PI/4.0)
            v_fi = M_PI/4.0;
    }
    if (key=='s')
    {
        v_fi -= M_PI/speed;
        if (v_fi < -M_PI/4.0)
            v_fi = -M_PI/4.0;
    }
    if (key=='d')
    {
        v_theta += M_PI/speed;
        if (v_theta > M_PI)
            v_theta -= 2.0*M_PI;
    }
    if (key=='a')
    {
        v_theta -= M_PI/speed;
        if (v_theta < -M_PI)
            v_theta += 2.0*M_PI;
    }
    if (key=='c')
        v_r -= 0.2;
    if (key=='z')
        v_r += 0.2;
    
    glutPostRedisplay();
}

void myReshape(int w, int h)
{
    glViewport(0, 0, w, h);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    
    ww = w;
    wh = h;
    
    if (w <= h)
        glFrustum(-1.0, 1.0, -1.0 * (GLfloat) h / (GLfloat) w, 1.0 * (GLfloat) h / (GLfloat) w, 1.0, 20.0);
    else
        glFrustum(-1.0, 1.0, -1.0 * (GLfloat) w / (GLfloat) h, 1.0 * (GLfloat) w / (GLfloat) h, 1.0, 20.0);
    glMatrixMode(GL_MODELVIEW);
}

void initLighting(void)
{
    // Enable lighting and specifiy model
    glShadeModel(GL_FLAT);
    glEnable(GL_LIGHTING);
    glEnable(GL_LIGHT0);
    //glEnable(GL_COLOR_MATERIAL);
    glLightModeli(GL_LIGHT_MODEL_LOCAL_VIEWER, GL_TRUE);
    //glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, GL_TRUE);
    
    // Define light properties
    GLfloat light_specular[] = {1.0, 1.0, 1.0, 1.0};
    GLfloat light_ambient[] = {0.3, 0.3, 0.3, 1.0};
    GLfloat light_diffuse[] = {1.0, 1.0, 1.0, 1.0};
    
    // Set light properties
    glLightfv(GL_LIGHT0, GL_POSITION, light_pos);
    glLightfv(GL_LIGHT0, GL_SPECULAR, light_specular);
    glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
    glLightfv(GL_LIGHT0, GL_DIFFUSE, light_diffuse);
    //glLightf(GL_LIGHT0, GL_CONSTANT_ATTENUATION, 1.0f);
    //glLightf(GL_LIGHT0, GL_LINEAR_ATTENUATION, 0.2f);
    //glLightf(GL_LIGHT0, GL_QUADRATIC_ATTENUATION, 0.04f);
    
    // Define material properties
    GLfloat mat_specular[] = {1.0, 1.0, 1.0, 1.0};
    GLfloat mat_ambient[] = {1.0, 1.0, 1.0, 1.0};
    GLfloat mat_diffuse[] = {1.0, 1.0, 1.0, 1.0};
    GLfloat mat_shininess = 100.0;
    
    // Set material properties
    glMaterialfv(GL_FRONT, GL_SPECULAR, mat_specular);
    glMaterialfv(GL_FRONT, GL_AMBIENT, mat_ambient);
    glMaterialfv(GL_FRONT, GL_DIFFUSE, mat_diffuse);
    glMaterialf(GL_FRONT, GL_SHININESS, mat_shininess);
}

int main(int argc, char** argv)
{
    //glutInit(&argc, argv);
    glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
    glutInitWindowSize(ww, wh);
    glutCreateWindow("Payload");
    glutReshapeFunc(myReshape);
    glutDisplayFunc(display);
    
    initLighting();
    
    int n = 60; // refresh rate
    glutTimerFunc(100, spinCube, n);
    //glutIdleFunc(spinCube);
    
    glutKeyboardFunc(keysDown);
    
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_CULL_FACE);
    
    glEnable(GL_TEXTURE_2D);
    glGenTextures(7, tex);
    for (int i = 0; i < 7; i++)
    {
        sf::Image image;
        string fileName = "Payload";
        fileName += static_cast<char>(i+48);
        fileName += ".jpg";
        if (i == 6)
            fileName = "Grass.jpg";
        if( !image.LoadFromFile(fileName) ) {
            std::cerr<<"Load image error!"<<std::endl;
            exit(1);
        }
        glBindTexture(GL_TEXTURE_2D, tex[i]);
        glTexImage2D( GL_TEXTURE_2D, 0, GL_RGBA, image.GetWidth(), image.GetHeight(),
                     0, GL_RGBA, GL_UNSIGNED_BYTE, (const GLvoid*)image.GetPixelsPtr() );
        glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_REPEAT);
        glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_REPEAT);
        glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);
        glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
        glHint(GL_PERSPECTIVE_CORRECTION_HINT,GL_NICEST);
    }
    
    mag_world.Normalize();
    Vector3f perp = mag_world.Cross(acc_world);
    mag_world = acc_world.Cross(perp);
    mag_world.Normalize();
    
    fin.open("Out.txt",ios::in);
    fin >> curr_time;
    
    glutMainLoop();
}

